#include <math.h>

#define PI 3.1415926535897932384626433832795

// Lovingly adapted from http://www.deltatau.com/common/support/technotes/SCARA%20Robot%20Kinematics.pdf
class ScaraSolver {

private:
	double l1, l2, l1_sq, l2_sq;

public:

	// initialize with lengths from shoulder to elbow (l1) and from elbow to wrist (l2)
	ScaraSolver(double l1_in, double l2_in) {
		l1 = l1_in; l2 = l2_in;
		l1_sq = pow(l1, 2); l2_sq = pow(l2, 2);
	}

	// gives angles for shoulder (a1) and elbow (a2) based on the desired x and y location
	// the origin is defined relative to the first joint
	void solve(double x, double y, double* a1, double* a2) {
		double x_sq_y_sq = pow(x, 2) + pow(y, 2);
		*a1 = atan2(y, x) - acos( (x_sq_y_sq + l1_sq - l2_sq) / (2 * l1 * sqrt(x_sq_y_sq)) );
		*a2 = PI - acos( (x_sq_y_sq - l1_sq - l2_sq) / (2 * l1 * l2) );
	}

};